/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "virtuallinecontrol.h"
#include "coordinatetrajectory.h"
#include "axisobject.h"
#include <QDebug>
#include "magnetictrack.h"
#include "domparser.h"
#include "ultrasonicsensor.h"
#include "lasersafetydetect.h"
#include "virtuallinebias.h"
#include "robotio.h"


#define D_DISTANCE_NOT_TRANSMOVE 0.025

extern UltrasonicSensor *g_ultrasonicSensor;

VirtualLineControl::VirtualLineControl(QString configFileIn, int robotIdIn, RobotCommonParameter *roboParameterIn,
                                       MotionMessage* motionMessageIn, MagneticTrack* maganeticTrackIn,
                                       E_VEHICLE_TYPE vehicleTypeIn, RobotIo *ioControllerIn)
{
    deltaTime=roboParameterIn->deltaTime;
    robotId=robotIdIn;
    motionMessage=motionMessageIn;
    maganeticTrack=maganeticTrackIn;
    vehicleType=vehicleTypeIn;
    ioController=ioControllerIn;
    pathControlStrategy= E_PATH_CONTROL_STRATEGY_DIFFERENTIAL;
    isServOn=false;
    isLaserLose=false;

    pathMovingInfo.passedStationId=-1;
    pathMovingInfo.nextStationId=-1;
    pathMovingInfo.currentStationId=-1;
    pathMovingInfo.passedPathId=-1;
    pathMovingInfo.nextPathId=-1;
    pathMovingInfo.currentPathId=-1;
    barrierProtectRatio=1.0;

    motionDebugInfo.commandQueueSize=0;
    motionDebugInfo.isVelprofileEmpty=1;

    basicStatus=E_VIRTUAL_LINE_CONTROL_STOPPED;
    targetWaitTime=roboParameterIn->agv_targetWaitTime;
    agv_allowLineOffset=roboParameterIn->agv_allowLineOffset/1000.0;
    agv_allowAngleOffset=roboParameterIn->agv_allowAngleOffset*M_PI/180.0;
    agv_allowLineMovingOffset=roboParameterIn->agv_allowLineMovingOffset/1000.0;
    agv_allowLineMovingOffset_ratio=roboParameterIn->agv_allowLineMovingOffset_ratio;
    agv_allowAngleMovingOffset=roboParameterIn->agv_allowAngleMovingOffset*M_PI/180.0;
    updateTrajStage=0;

    JointAttribute axisAttributeIn;
    axisAttributeIn.maxVelocity=roboParameterIn->agv_lineMaxSpeed; //unit is mm/s
    axisAttributeIn.maxAccelation=roboParameterIn->agv_lineMaxAcceleration;
    coordinateTrajectory=new CoordinateTrajectory(-1,deltaTime,axisAttributeIn,motionMessageIn,
                          roboParameterIn->rotate_eqradius,roboParameterIn->allowVelFeedbackBiasRatio,
                                                  roboParameterIn->agv_rotateMaxSpeed,
                                                  roboParameterIn->agv_rotateMaxAcceleration);
    lineAcc_slowdown=roboParameterIn->agv_emergencyLineAcceleration/1000.0;
    rotateAcc_slowdown=roboParameterIn->agv_emergencyRotateAcceleration/1000.0;

    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    QString configFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/vehicle.ini";
    virtualLineBias=new VirtualLineBias(configFile,deltaTime);
    if(true!=virtualLineBias->isInitialOk())
    {
       qDebug()<<"error, VirtualLineControl virtualLineBias";
       addMsg(ENUM_MSG_ERROR,"MotionServer","VirtualLineControl",8109,robotId);
    }
    bool isSteeringValid=false;
    if(E_VEHICLE_TYPE_ONE_STERRING_WHEEL==vehicleTypeIn)
    {
        isSteeringValid=true;
    }
    coordinateTrajectory->setSteeringMotionParameter(virtualLineBias->getSteeringMaxSpeed(),
                                                     virtualLineBias->getSteeringMaxAcceration(),isSteeringValid);

    initialPointCloudSafeDetect();

    //---------初始化nav线程锁--------
    pthread_mutexattr_init(&navAttr);
    pthread_mutexattr_setprotocol(&navAttr,PTHREAD_PRIO_INHERIT);
    pthread_mutex_init(&navMutex,&navAttr);

    resetTrajTime();
    isTrajectoryContiousRun=false;

    timeAxisScale=0.0;
    axisAccelerationRatio=0;

    currentLine=ENUM_LINE_INITIAL;
    isCurrentLineFinished=-1;
    currentLineRemainingTime=-1;

    timeAxisParameter.slowDownRatio=0.1;
    timeAxisParameter.startAcceleration=0.2;
    timeAxisParameter.startDuration=3;
    timeAxisParameter.stopAcceleration=0.6;
    timeAxisParameter.stopDuration=5;

    timeAxisStatus=ENUM_TIME_AXIS_NORMAL;
    oldTimeAxisRatio=0.0;
    newTimeAxisRatio=0.0;

    totalSpeedRatio=1;
    safeSpeedRatio=1;
    controlSpeedRatio=0;
    pidSpeedRatio=1;
    adjustSpeedRatio=1;

    debugFlag=0;


    initialTimeAxis();
    initialVirtualAxis();

    QString readComent;
    int tmpKey=readConfigFile(configFileIn,readComent);
    if(1!=tmpKey)
    {
       qDebug()<<"error, VirtualLineControl readConfigFile";
       addMsg(ENUM_MSG_ERROR,"MotionServer","VirtualLineControl",8108,robotId,0,0,0,0,readComent);
    }
    else
    {
        coordinateTrajectory->setNoRatateMoveAngle(noRotateMoveAngle);
    }
}

int VirtualLineControl::controlTypeSwitch(int controlTypeIn)
{
    return laserSafetyDetect->controlTypeSwitch(controlTypeIn);
}

int VirtualLineControl::getVehicleTrajectoryControlParameter(VehicleTrajectoryControlParameter &infoOut)
{
    return coordinateTrajectory->getVehicleTrajectoryControlParameter(infoOut);
}

int VirtualLineControl::setVehicleTrajectoryControlParameter(VehicleTrajectoryControlParameter infoIn, int &returnFlag)
{
    return coordinateTrajectory->setVehicleTrajectoryControlParameter(infoIn,returnFlag);
}

int VirtualLineControl::getAgvBorderInfo(VehicleBorderInfo &infoOut)
{

    return laserSafetyDetect->getAgvBorderInfo(infoOut);
}

int VirtualLineControl::setAgvBorderRectInfo(BorderRectInfo rectInfoIn, int &returnFlag)
{

    return laserSafetyDetect->setAgvBorderRectInfo(rectInfoIn,returnFlag);
}

int VirtualLineControl::getAgvSheftInfo(int sheftIndex, BorderRectInfo &infoOut)
{
    return laserSafetyDetect->getAgvSheftInfo(sheftIndex,infoOut);
}

int VirtualLineControl::setAgvSheftRectInfo(int sheftIndex, BorderRectInfo rectInfoIn, int &returnFlag)
{
    return laserSafetyDetect->setAgvSheftRectInfo(sheftIndex,rectInfoIn,returnFlag);
}

double VirtualLineControl::getBarrierProtectRatioOfMoving()
{
    return barrierProtectRatio;
}

int VirtualLineControl::setPathMovingInfo_virtualLineControl(PathMovingInfo pathInfoIn)
{
//    qDebug()<<"setPathMovingInfo_virtualLineControl passedStationId"<<pathInfoIn.passedStationId
//           <<"currentStationId"<<pathInfoIn.currentStationId;
//    pathMovingInfo.passedStationId=passedStationId;
//    pathMovingInfo.currentStationId=currentStationId;

//    pathMovingInfo.nextStationId=-1;
//    pathMovingInfo.passedPathId=-1;
//    pathMovingInfo.nextPathId=-1;
//    pathMovingInfo.currentPathId=-1;
    pathMovingInfo=pathInfoIn;
    return 1;
}

int VirtualLineControl::getPathMovingInfo_virtualLineControl(PathMovingInfo &infoOut)
{
    //todo
    infoOut=pathMovingInfo;
    return 1;
}

int VirtualLineControl::getTrajectoryPoint_steeringAngle(QVector<double> &steeringAngleOut)
{
    steeringAngleOut.resize(1);
    steeringAngleOut[0]=coordinateTrajectory->getTrajectoryPoint_steeringAngle();
    return 1;
}

E_BIAS_TYPE VirtualLineControl::getCalculateBiasType()
{
    return virtualLineBias->getCalculateBiasType();
}

int VirtualLineControl::getCurrentPathInformation(VehiclePosition &startPositionOut, QVector<VehiclePosition> &targetPositionOut,
                       int &pointIndexOut, E_NAV_COMMAND &currentPathTypeOut, double &currentPathParameter1Out,
                                                  double &currentPathParameter2Out)
{
    return coordinateTrajectory->getCurrentPathInformation(startPositionOut,targetPositionOut,pointIndexOut,currentPathTypeOut,
                                                           currentPathParameter1Out,currentPathParameter2Out);
}

E_BARRIER_RUN_MODE VirtualLineControl::getBarrierRunMode()
{
    return laserSafetyDetect->getBarrierRunMode();
}

int VirtualLineControl::setBarrierRunMode(E_BARRIER_RUN_MODE modeIn)
{
    qDebug()<<"VirtualLineControl::setBarrierRunMode"<<modeIn;
    if(E_BARRIER_RUN_MODE_NONE==modeIn)
    {
        setUltrasonicSensorCheckEnable(false);
        setPointCloudCheckEnable(false);
    }
    else if(E_BARRIER_RUN_MODE_DONOTHING==modeIn)
    {
        return 1;
    }
    else
    {
        setUltrasonicSensorCheckEnable(true);
        setPointCloudCheckEnable(true);
    }
    return laserSafetyDetect->setBarrierRunMode(modeIn);
}

int VirtualLineControl::setLaserLose(bool isLose,int positionType)
{
    isLaserLose=isLose;
    if(true==isLaserLose)
    {
        if(0==positionType)
        {
            setSafeSpeedRatio(0,3);
        }
        else
        {
            setSafeSpeedRatio(0,0.05);
        }

    }
    else
    {
        setSafeSpeedRatio(1,5);
    }

}

int VirtualLineControl::setDebugFlag(int flagIn)
{
    debugFlag=flagIn;
    virtualLineBias->setDebugFlag(flagIn);
    return 1;
}

int VirtualLineControl::setAgvPathFollowStrategy(E_PATH_CONTROL_STRATEGY stragetyIn)
{
    pathControlStrategy=stragetyIn;
    virtualLineBias->setAgvPathFollowStrategy(stragetyIn);
    return 1;
}

int VirtualLineControl::getBarriorDetectMode(E_BARRIER_TETECT_MODE &modeOut)
{
    modeOut= laserSafetyDetect->getDetectMode();
    return 1;
}

int VirtualLineControl::getSafeCollisionInfo(int sensorType, CollisionPara &infoOut)
{
    return laserSafetyDetect->getSafeCollisionInfo(sensorType,infoOut);
}

int VirtualLineControl::setSafeCollisionInfo(int sensorType, CollisionPara infoIn, int &returnFlag)
{
    return laserSafetyDetect->setSafeCollisionInfo(sensorType,infoIn,returnFlag);
}

int VirtualLineControl::setCurrentVehicleBorderId(int idIn)
{
    return laserSafetyDetect->setCurrentVehicleBorderId(idIn);
}

int VirtualLineControl::getCurrentVehicleBorderId()
{
    return laserSafetyDetect->getCurrentVehicleBorderId();
}

int VirtualLineControl::setVirtualLineEnableKpChange(bool isEnable)
{
    return  virtualLineBias->setVirtualLineEnableKpChange(isEnable);
}

int VirtualLineControl::getSafeSpeedRatio(double &ratioOut)
{

//    if(E_VIRTUAL_LINE_CONTROL_RUN!=basicStatus)
//    {
//        safeSpeedRatio=1;
//    }
    ratioOut=safeSpeedRatio;
    return 1;
}

int VirtualLineControl::setTargetAllowOffset(float allowLineOffsetIn, float allowAngleOffsetIn)
{
    agv_allowAngleOffset=allowAngleOffsetIn;
    agv_allowLineOffset=allowLineOffsetIn;
    return 1;
}

int VirtualLineControl::getTimeConsumed_pointCloudDetect(double &minConsumedOut, double &currentConsumedOut, double &maxConsumedOut)
{
    return laserSafetyDetect->getTimeConsumed(minConsumedOut,currentConsumedOut,maxConsumedOut);
}

int VirtualLineControl::setVirtualLinePid(double kpDistanceErrorIn, double kpAngleDistanceIn,
                                          double kpOnlyRotationIn, double kpDistanceError_yIn)
{
    return virtualLineBias->setVirtualLinePid(kpDistanceErrorIn,kpAngleDistanceIn,kpOnlyRotationIn,kpDistanceError_yIn);
}

int VirtualLineControl::getTargetCommandVehiclePosition(VehiclePosition &targetOut,int &pointIndexOut)
{
    return coordinateTrajectory->getVehicleTargetPosition(targetOut,pointIndexOut);
}

int VirtualLineControl::getCarBorder(std::vector<VehiclePosition> &carBorderOut)
{
    if(NULL==laserSafetyDetect)
    {
        return -1;
    }
    return laserSafetyDetect->getCarBorder(carBorderOut);
}

int VirtualLineControl::getCarBorderOffset(std::vector<VehiclePosition> &carBorderOffsetOut)
{
    if(NULL==laserSafetyDetect)
    {
        return -1;
    }
    return laserSafetyDetect->getCarBorderOffset(carBorderOffsetOut);
}

int VirtualLineControl::setPointCloudData(const QVector<PointCloudData_3D> &pointDataIn)
{
//    return 1;//debug test1,4day not problem
    if(NULL==laserSafetyDetect)
    {
        return -1;
    }
    return laserSafetyDetect->setPointCloudData(pointDataIn);
}

int VirtualLineControl::setVehicleBorderOffset(double front_offsetIn, double back_offsetIn, double left_offsetIn,
                                               double right_offsetIn)
{
    if(NULL==laserSafetyDetect)
    {
        return -1;
    }
    return laserSafetyDetect->setVehicleBorderOffset(front_offsetIn,back_offsetIn,left_offsetIn,right_offsetIn);
}

int VirtualLineControl::setUltrasonicSensorCheckEnable(bool isOn)
{
    if(NULL==g_ultrasonicSensor)
    {
        return 0;
    }
    return g_ultrasonicSensor->setUltrasonicSensorCheckEnable(isOn);
}

int VirtualLineControl::getUltrasonicSensorSafeStatus(std::vector<double> &safeStatusOut, bool &isCheckEnableOut)
{
    if(NULL==g_ultrasonicSensor)
    {
        return 0;
    }
    return g_ultrasonicSensor->getUltrasonicSensorSafeStatus(safeStatusOut,isCheckEnableOut);
}

int VirtualLineControl::getPointCloudSafeStatus(std::vector<string> &sensorNameOut, std::vector<double> &sensorSafeRatioOut, bool &isCheckEnableOut)
{
    if(NULL==laserSafetyDetect)
    {
        return -1;
    }
    return laserSafetyDetect->getPointCloudSafeStatus(sensorNameOut,sensorSafeRatioOut,isCheckEnableOut);
}

int VirtualLineControl::setPointCloudCheckEnable(bool isOn)
{
    if(NULL==laserSafetyDetect)
    {
        return -1;
    }
    return laserSafetyDetect->setPointCloudCheckEnable(isOn);
}

int VirtualLineControl::setAcceleration(double accelerationIn)
{
    pthread_mutex_lock(&navMutex);
    coordinateTrajectory->setAcceleration(accelerationIn);
    pthread_mutex_unlock(&navMutex);
    return 1;
}

int VirtualLineControl::setAcceleration_rotate(double accelerationIn)
{
    pthread_mutex_lock(&navMutex);
    coordinateTrajectory->setAcceleration_rotate(accelerationIn);
    pthread_mutex_unlock(&navMutex);
    return 1;
}

int VirtualLineControl::setServoStatus(bool isOn)
{
    isServOn=isOn;
    return 1;
}

int VirtualLineControl::startRun()
{

    basicStatus=E_VIRTUAL_LINE_CONTROL_RUN;
//    setTimeAxisScale(1,2);

    return 1;

}

int VirtualLineControl::cease()
{
    if(E_VIRTUAL_LINE_CONTROL_CEASING==basicStatus)
    {
       return -1;
    }
    basicStatus=E_VIRTUAL_LINE_CONTROL_TOCEASE;



    double standardSlowTime=2;
    double tmpCompareTime=calculateSlowDownTime();
    if(tmpCompareTime<standardSlowTime)
    {
       setTimeAxisScale(0,tmpCompareTime);
       qDebug()<<"cease use tmpCompareTime"<<tmpCompareTime;
    }
    else
    {
        setTimeAxisScale(0,standardSlowTime);
    }

    basicStatus=E_VIRTUAL_LINE_CONTROL_CEASING;
    return 1;

}

int VirtualLineControl::quickStop()
{
    resetAll_exceptStatus();
//    resetAll();//清除旧轨迹,重新生产轨迹。
    basicStatus=E_VIRTUAL_LINE_CONTROL_CEASED;
    return 1;
}


int VirtualLineControl:: initialVirtualAxis()
{
    //轴初始化
    JointAttribute attribute;
    //trap用
    attribute.maxAccelation=6;
    //attribute.maxVelocity=1.7;
    attribute.maxVelocity=3;
    //spline用
//        attribute.maxAccelation=6;
//        attribute.maxVelocity=2;

    //trap用
    attribute.maxAccelation=3;
    //attribute.maxVelocity=1.7;
    attribute.maxVelocity=10;

    attribute.maxTorque=10000;
    attribute.maxPosition=1.1;
    attribute.minPosition=-0.1;
    attribute.coupleFlag=0;
    attribute.coupleJoint=0;
    attribute.coupleReducer=0;
    attribute.jointType=0;
    attribute.pulsePerRound=100;
    attribute.externAxisFlag=0;
    attribute.gearRatio=1;
    attribute.jointName="speedRatioAxis";
    attribute.mobile=false;
    attribute.processFollowError=2;
    attribute.screwPitch=1;
    attribute.targetFollowError=1;

    virtualAxis=new AxisObject(1,attribute,motionMessage);
    virtualAxis->axisIdByRobot=1;
    virtualAxis->deltaTime=deltaTime;

    return 1;
}

int VirtualLineControl::resetAll()
{
    if(2==debugFlag)//debugFlag
    {
        qDebug()<<"VirtualLineControl::resetAll()";
    }
    currentLine=ENUM_LINE_INITIAL;
    basicStatus=E_VIRTUAL_LINE_CONTROL_STOPPED;
    resetAll_exceptStatus();

}

int VirtualLineControl::resetAll_exceptStatus()
{
    virtualLineBias->setCalculateBiasType_noDebug(E_BIAS_TYPE_NONE);

    exceedTimeCount=0;
    pthread_mutex_lock(&navMutex);
    commandQueue.clear();
    timeAxis->resetAll();
    pthread_mutex_unlock(&navMutex);
    synchronizeMotorFeedbackToCalculatePosition();
    resetTrajTime();
    absoluteDistance_old=0;
    absoluteAngle_old=0;
    stopOutput();
    laserSafetyDetect->setMotionStatus(E_MOTION_ID_NONE);
}

int VirtualLineControl::addTrajectoryCommand(QVector<NavParameter> commandVectorIn)
{
//    if( basicStatus==ENUM_TRAJECTORY_STOPPING)
//    {
//        return -1;
//    }

    printInformation(commandVectorIn,"addNavCommand");
    pthread_mutex_lock( &navMutex);//写运动指令时加锁
    for(int i = 0; i < commandVectorIn.size();i++)
    {
        commandQueue.enqueue(commandVectorIn[i]);
    }
    pthread_mutex_unlock( &navMutex);//写完后解锁
    return 1;
}

int VirtualLineControl::printInformation(NavParameter moveParameter, QString title)
{
    if(2==debugFlag)//debugFlag
    {
    qDebug() <<"-------------------------------------"<< title <<"------------------------------";
    qDebug() << "******"<<"===acc=="<<moveParameter.acc
                        <<"===cntRatio=="<<moveParameter.cntRatio
                        <<"==dpIndex==="<<moveParameter.targetPoint.dpIndex
                        <<"==lineNumber==="<<moveParameter.lineNumber
                        <<"==moveType==="<<moveParameter.moveType
                        <<"===velocityUnit=="<<moveParameter.velocityUnit
                        <<"===velocity=="<<moveParameter.velocity
                          <<"===posX=="<<moveParameter.targetPoint.posX
                            <<"===posY=="<<moveParameter.targetPoint.posY
                             <<"===posR=="<<moveParameter.targetPoint.posR
                                 <<"===pointType=="<<moveParameter.targetPoint.pointType
                                   <<"===isPositiveDirection=="<<moveParameter.isPositiveDirection
                                <<"===p1x=="<<moveParameter.p1x
                                  <<"===p1y=="<<moveParameter.p1y
                                    <<"===p2x=="<<moveParameter.p2x
                                      <<"===p2y=="<<moveParameter.p2y;

    }

       return 1;

}


int VirtualLineControl::printInformation(QVector<NavParameter> moveParameterArry, QString title)
{
    if(2==debugFlag)
    {
        qDebug() <<"-------------------------------------"<< title <<"------------------------------";
        int size=moveParameterArry.size();
        for(int i=0;i<size;i++)
        {
            qDebug() << "/////////////"<<"command index"<<i;
            qDebug() << "******"<<"===acc=="<<moveParameterArry[i].acc
                                <<"===cntRatio=="<<moveParameterArry[i].cntRatio
                                <<"==dpIndex==="<<moveParameterArry[i].targetPoint.dpIndex
                                <<"==lineNumber==="<<moveParameterArry[i].lineNumber
                                <<"==moveType==="<<moveParameterArry[i].moveType
                                <<"===velocityUnit=="<<moveParameterArry[i].velocityUnit
                                <<"===velocity=="<<moveParameterArry[i].velocity
                                  <<"===posX=="<<moveParameterArry[i].targetPoint.posX
                                    <<"===posY=="<<moveParameterArry[i].targetPoint.posY
                                     <<"===posR=="<<moveParameterArry[i].targetPoint.posR
                                         <<"===pointType=="<<moveParameterArry[i].targetPoint.pointType
                                           <<"===p1x=="<<moveParameterArry[i].p1x
                                             <<"===p1y=="<<moveParameterArry[i].p1y
                                               <<"===p2x=="<<moveParameterArry[i].p2x
                                                 <<"===p2y=="<<moveParameterArry[i].p2y;
        }
    }


    return 1;
}


int VirtualLineControl::calculateTrajectoryPath()
{
    motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH;
    NavParameter moveParameter;
    if(0==getCommandFromQueue(moveParameter))
    {
        //qDebug()<<"TrajectoryServer::calculateTrajectoryPath(), return 0";
        motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH_NOCOMMAND;
        motionDebugInfo.commandQueueSize=0;
        return 0;
    }
    motionDebugInfo.commandQueueSize=getCommandQueueSize();

    int processResult=-1;
    int queueGetedFlag=0;
    QVector<NavParameter> moveParameterArry;
    while(-1==processResult)//如果计算轨迹的时候还有旧轨迹，就会等。
    {
        motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH_WHILE;
        switch(moveParameter.moveType)
        {

            case E_NAV_COMMAND_LINE:
            {
                if(0==moveParameter.cntRatio)
                {//单条
                    processResult=processNavLineInstruction(moveParameter);
                    break;
                }
                else
                {//多条平滑
                    addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",8118,robotId);
                    qDebug()<<"不支持直线平滑";
                    return -1;

                    if(queueGetedFlag==0)
                    {
                        moveParameterArry.append(moveParameter);

                        while(0!=getCommandFromQueue(moveParameter) && 0!=moveParameter.cntRatio)
                        {
                           moveParameterArry.append(moveParameter);
                        }
                        if(0==moveParameter.cntRatio)
                        {
                           moveParameterArry.append(moveParameter);
                        }

                        queueGetedFlag=1;
                    }
                    processResult=processNavLineContinusInstruction(moveParameterArry,0);

                     break;
                }
            }


            case E_NAV_COMMAND_SPLINE:
            {
                if(0==moveParameter.cntRatio)
                {//单条
                    processResult=processNavSplineInstruction(moveParameter);
                    if(0>processResult)
                    {
                        return processResult;
                    }
                    break;
                }
                else
                {//多条平滑

                    if(queueGetedFlag==0)
                    {
                        moveParameterArry.append(moveParameter);

                        while(0!=getCommandFromQueue(moveParameter) && 0!=moveParameter.cntRatio)
                        {
                           moveParameterArry.append(moveParameter);
                        }
                        if(0==moveParameter.cntRatio)
                        {
                           moveParameterArry.append(moveParameter);
                        }

                        queueGetedFlag=1;
                    }
                    processResult=processNavSplineContinusInstructionWithControlPoints(moveParameterArry);

                     break;
                }
            }
        case E_NAV_COMMAND_ROTATE:
        {

                processResult=processNavRotateInstruction(moveParameter);
                break;

        }

            default:
            {
                addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",8113,robotId,moveParameter.moveType);
                qDebug()<<"not match error code";
                return -1;
            }

        }

        //updateTrajTime(), 偶尔出现，行号确实完成了,当是trajProfile还没有来得及清除，导致添加失败。
//        if(-1==processResult)
//        {
//            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5021,robotId);
//            return -1;
//        }
        // 停止的时候跳出
        usleep(1000);
        if(basicStatus==E_VIRTUAL_LINE_CONTROL_STOPPING||basicStatus==E_VIRTUAL_LINE_CONTROL_STOPPED)
        {
            motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH_WHILE_BREAK;
            return -1;
        }
    }
    motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH_NORMAL_FINISH;
    return 1;
}



int VirtualLineControl::processNavLineInstruction(NavParameter moveParameter)
{
    printInformation(moveParameter,"processNavLineInstruction");

    setVehicleDirection(moveParameter.isPositiveDirection);//前进和倒退方向参数处理问题
    int tmpReturn=1;

    double timeDurationOut=0;

    pthread_mutex_lock(&navMutex);
    //axisGroup->resetCoordinateTraj(0);
//    updatePositionFrameCommand();
    VehiclePosition tmpPos;
    dpointToVehiclePosition(moveParameter.targetPoint,tmpPos);
    QVector<VehiclePosition> tmpPointList;
    tmpPointList.append(tmpPos);
    coordinateTrajectory->setPathInformation(tmpPointList,moveParameter.targetPoint.dpIndex,E_NAV_COMMAND_LINE);
    coordinateTrajectory->synchronizePositionCommandWithFeedback();
    double tmpDistance,tmpAngle;
    coordinateTrajectory->getAbsoluteVehiclePositionLeft(tmpDistance,tmpAngle);
    if(noTransMoveDistance>tmpDistance)
    {
        qDebug()<<"processNavLineInstruction not moved,tmpDistance"<<tmpDistance;

    }
    else
    {


        if(E_PATH_CONTROL_STRATEGY_DIFFERENTIAL==pathControlStrategy)
        {
            //spline 速度太慢了
      //    tmpReturn=coordinateTrajectory->lineMove(targetFrame,moveParameter.velocity,timeDurationOut);//test1 ok
             tmpReturn=coordinateTrajectory->lineMove_3segment_composed(moveParameter.isPositiveDirection,true,
                                                         tmpPos,moveParameter.velocity,timeDurationOut,false);//
        }
        else if(E_PATH_CONTROL_STRATEGY_MCCRUM==pathControlStrategy)
        {
            //设置理想姿态
            virtualLineBias->setMaccrumIdealMovingLineAngle(tmpPos.rotate);

             tmpReturn=coordinateTrajectory->lineMove_maccrum_2segment_composed(false,
                                                    tmpPos,moveParameter.velocity,timeDurationOut);//
        }
    }




    pthread_mutex_unlock(&navMutex);
    if(1!=tmpReturn)
    {
        addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8100,robotId);
//        qDebug()<<"processNavLineInstruction error,1!=tmpReturn";
        return -1;
    }


    pthread_mutex_lock(&navMutex);
    addTrajTime(moveParameter.lineNumber,moveParameter.stationId,moveParameter.pathId,timeDurationOut);
    pthread_mutex_unlock(&navMutex);

    return 1;
}



int VirtualLineControl::addTrajTime(int lineNumber,int stationIdIn,int pathIdIn, double timeDuration)//调用者做锁保护
{
    lineQueue.enqueue(lineNumber);
    timeDurationQueue.enqueue(timeDuration);
    stationIdQueue.enqueue(stationIdIn);
    pathIdQueue.enqueue(pathIdIn);
    return 1;
}



int VirtualLineControl::processNavLineContinusInstruction(QVector<NavParameter> moveParameterArry, int isLastPointMove)
{
    printInformation( moveParameterArry, "processNavLineContinusInstruction");
    if(0==moveParameterArry.size())
    {
        return -1;
    }

    int pointCount=moveParameterArry.size();
    setVehicleDirection(moveParameterArry[0].isPositiveDirection);
    int tmpReturn=0;
    QVector<VehiclePosition> targetFrame;
    targetFrame.resize(pointCount);
    QVector<double> speedPercent,timeDurationResult,cntRatio;
    speedPercent.resize(pointCount);
    timeDurationResult.resize(pointCount);
    cntRatio.resize(pointCount);
    for(int i=0;i<pointCount;i++)
    {
       dpointToVehiclePosition(moveParameterArry[i].targetPoint,targetFrame[i]);;
       speedPercent[i]=moveParameterArry[i].velocity;
       cntRatio[i]=moveParameterArry[i].cntRatio;
    }

    pthread_mutex_lock(&navMutex);
//    updatePositionFrameCommand();
    coordinateTrajectory->setPathInformation(targetFrame,moveParameterArry.last().targetPoint.dpIndex,E_NAV_COMMAND_LINE);
    coordinateTrajectory->synchronizePositionCommandWithFeedback();
    if(E_PATH_CONTROL_STRATEGY_DIFFERENTIAL==pathControlStrategy)
    {
        tmpReturn=coordinateTrajectory->lineContinuousMove_vehiclePosition(moveParameterArry[0].isPositiveDirection,
                true,targetFrame,speedPercent,timeDurationResult,cntRatio);
    }
    else if(E_PATH_CONTROL_STRATEGY_MCCRUM==pathControlStrategy)
    {
        //设置理想姿态
        virtualLineBias->setMaccrumIdealMovingLineAngle(targetFrame.last().rotate);
        tmpReturn=coordinateTrajectory->lineContinuousMove_vehiclePosition_maccrum(true,targetFrame,
                                                                speedPercent,timeDurationResult,cntRatio);
    }

    pthread_mutex_unlock(&navMutex);
    if(1!=tmpReturn)
    {
        addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8101,robotId);
        qDebug()<<"processNavLineContinusInstruction error,1!=tmpReturn";
        return -1;
    }


    pthread_mutex_lock(&navMutex);
    for(int i=0;i<pointCount;i++)
    {
       addTrajTime(moveParameterArry[i].lineNumber,moveParameterArry[i].stationId,moveParameterArry[i].pathId,timeDurationResult[i]);
    }
    pthread_mutex_unlock(&navMutex);

    return 1;
}

int VirtualLineControl::processNavSplineInstruction(NavParameter moveParameter)
{
    qDebug()<<"VirtualLineControl::processNavSplineInstruction";

    printInformation(moveParameter,"processNavSplineInstruction");

    setVehicleDirection(moveParameter.isPositiveDirection);//前进和倒退方向参数处理问题
    int tmpReturn=1;

    double timeDurationOut;

    pthread_mutex_lock(&navMutex);
    //axisGroup->resetCoordinateTraj(0);
//    updatePositionFrameCommand();
    VehiclePosition tmpTargetPoint,tmpMiddle1,tmpMiddle2;
    dpointToVehiclePosition(moveParameter.targetPoint,tmpTargetPoint);
    tmpMiddle1.x=moveParameter.p1x;
    tmpMiddle1.y=moveParameter.p1y;
    tmpMiddle2.x=moveParameter.p2x;
    tmpMiddle2.y=moveParameter.p2y;
    QVector<VehiclePosition> tmpPointList;
    tmpPointList.append(tmpTargetPoint);
//    tmpPointList.append(tmpMiddle1);
//    tmpPointList.append(tmpMiddle2);
    //D_BEZIER_LENGTH_RATIO
    coordinateTrajectory->setPathInformation(tmpPointList,moveParameter.targetPoint.dpIndex,E_NAV_COMMAND_SPLINE,
                                             D_BEZIER_LENGTH_RATIO,moveParameter.isPositiveDirection);
    coordinateTrajectory->synchronizePositionCommandWithFeedback();
    double tmpDistance,tmpAngle;
    coordinateTrajectory->getAbsoluteVehiclePositionLeft(tmpDistance,tmpAngle);

    if(noTransMoveDistance>tmpDistance)
    {
        qDebug()<<"processNavSplineInstruction not moved,tmpDistance"<<tmpDistance;

    }
    else
    {
        if(E_PATH_CONTROL_STRATEGY_DIFFERENTIAL==pathControlStrategy)
        {
            //spline 速度太慢了
      //    tmpReturn=coordinateTrajectory->lineMove(targetFrame,moveParameter.velocity,timeDurationOut);//test1 ok
//             tmpReturn=coordinateTrajectory->bezierMove_noRotateFromCurrentPos(E_LINE_VELOCITY_STYLE_ZERO_ZERO,moveParameter.isPositiveDirection,
//                                                         tmpPos,moveParameter.velocity,timeDurationOut);//
            tmpReturn=coordinateTrajectory->bezierMove_withControlPoints(E_LINE_VELOCITY_STYLE_ZERO_ZERO,
                                                             moveParameter.isPositiveDirection,
                              tmpTargetPoint,tmpMiddle1,tmpMiddle2,moveParameter.velocity,timeDurationOut);
        }
        else if(E_PATH_CONTROL_STRATEGY_MCCRUM==pathControlStrategy)
        {
            addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8119,robotId);
            qDebug()<<"processNavSplineInstruction error,不支持轨迹策略";
            return -1;
            //设置理想姿态 NEED TODO
    //            virtualLineBias->setMaccrumIdealMovingLineAngle(tmpPos.rotate);

    //             tmpReturn=coordinateTrajectory->lineMove_maccrum_2segment_composed(false,
    //                                                    tmpPos,moveParameter.velocity,timeDurationOut);//
        }
    }




    pthread_mutex_unlock(&navMutex);
    if(1!=tmpReturn)
    {
//        addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8100,robotId);
//        qDebug()<<"processNavLineInstruction error,1!=tmpReturn";
        return tmpReturn;
    }


    pthread_mutex_lock(&navMutex);
    addTrajTime(moveParameter.lineNumber,moveParameter.stationId,moveParameter.pathId,timeDurationOut);
    pthread_mutex_unlock(&navMutex);

    return 1;
}

int VirtualLineControl::processNavSplineContinusInstruction(QVector<NavParameter> moveParameterArry)
{
    printInformation( moveParameterArry, "processNavSplineContinusInstruction");
    if(0==moveParameterArry.size())
    {
        return -1;
    }

    int pointCount=moveParameterArry.size();
    setVehicleDirection(moveParameterArry[0].isPositiveDirection);
    int tmpReturn=0;
    QVector<VehiclePosition> targetFrame;
    targetFrame.resize(pointCount);
    QVector<double> speedPercent,timeDurationResult,cntRatio;
    speedPercent.resize(pointCount);
    timeDurationResult.resize(pointCount);
    cntRatio.resize(pointCount);
    for(int i=0;i<pointCount;i++)
    {
       dpointToVehiclePosition(moveParameterArry[i].targetPoint,targetFrame[i]);;
       speedPercent[i]=moveParameterArry[i].velocity;
       cntRatio[i]=moveParameterArry[i].cntRatio;
    }

    pthread_mutex_lock(&navMutex);
//    updatePositionFrameCommand();
    coordinateTrajectory->setPathInformation(targetFrame,moveParameterArry.front().targetPoint.dpIndex,
                        E_NAV_COMMAND_SPLINE,D_BEZIER_LENGTH_RATIO,moveParameterArry.front().isPositiveDirection);
    coordinateTrajectory->synchronizePositionCommandWithFeedback();
    if(E_PATH_CONTROL_STRATEGY_DIFFERENTIAL==pathControlStrategy)
    {
        tmpReturn=coordinateTrajectory->splineContinuousMove_vehiclePosition(moveParameterArry[0].isPositiveDirection,
                true,targetFrame,speedPercent,timeDurationResult,cntRatio);
    }
    else if(E_PATH_CONTROL_STRATEGY_MCCRUM==pathControlStrategy)
    {
        addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8119,robotId);
        qDebug()<<"processNavSplineContinusInstruction error,不支持轨迹策略";
        return -1;
        //设置理想姿态
//        virtualLineBias->setMaccrumIdealMovingLineAngle(targetFrame.last().rotate);
//        tmpReturn=coordinateTrajectory->splineContinuousMove_vehiclePosition_maccrum(true,targetFrame,
//                                                                speedPercent,timeDurationResult,cntRatio);
    }

    pthread_mutex_unlock(&navMutex);
    if(1!=tmpReturn)
    {
        addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8101,robotId);
        qDebug()<<"processNavSplineContinusInstruction error,1!=tmpReturn";
        return -1;
    }


    pthread_mutex_lock(&navMutex);
    for(int i=0;i<pointCount;i++)
    {
       addTrajTime(moveParameterArry[i].lineNumber,moveParameterArry[i].stationId,
                   moveParameterArry[i].pathId,timeDurationResult[i]);
       qDebug()<<"addTrajTime"<<timeDurationResult[i];
    }
    pthread_mutex_unlock(&navMutex);

    return 1;
}

int VirtualLineControl::processNavSplineContinusInstructionWithControlPoints(QVector<NavParameter> moveParameterArry)
{
    printInformation( moveParameterArry, "processNavSplineContinusInstructionWithControlPoints");
    if(0==moveParameterArry.size())
    {
        return -1;
    }

    int pointCount=moveParameterArry.size();
    setVehicleDirection(moveParameterArry[0].isPositiveDirection);
    int tmpReturn=0;
    QVector<VehiclePosition> targetFrame,middle1,middle2;
    targetFrame.resize(pointCount);
    middle1.resize(pointCount);
    middle2.resize(pointCount);
    QVector<double> speedPercent,timeDurationResult,cntRatio;
    speedPercent.resize(pointCount);
    timeDurationResult.resize(pointCount);
    cntRatio.resize(pointCount);
    for(int i=0;i<pointCount;i++)
    {
       dpointToVehiclePosition(moveParameterArry[i].targetPoint,targetFrame[i]);;
       speedPercent[i]=moveParameterArry[i].velocity;
       cntRatio[i]=moveParameterArry[i].cntRatio;
       middle1[i].x=moveParameterArry[i].p1x;
       middle1[i].y=moveParameterArry[i].p1y;
       middle2[i].x=moveParameterArry[i].p2x;
       middle2[i].y=moveParameterArry[i].p2y;
    }

    pthread_mutex_lock(&navMutex);
//    updatePositionFrameCommand();
    coordinateTrajectory->setPathInformation(targetFrame,moveParameterArry.front().targetPoint.dpIndex,
                        E_NAV_COMMAND_SPLINE,D_BEZIER_LENGTH_RATIO,moveParameterArry.front().isPositiveDirection);
    coordinateTrajectory->synchronizePositionCommandWithFeedback();
    if(E_PATH_CONTROL_STRATEGY_DIFFERENTIAL==pathControlStrategy)
    {
        tmpReturn=coordinateTrajectory->splineContinuousMove_vehiclePositionWithControlPoints(
                    moveParameterArry[0].isPositiveDirection,
                true,targetFrame,middle1,middle2,speedPercent,timeDurationResult,cntRatio);
    }
    else if(E_PATH_CONTROL_STRATEGY_MCCRUM==pathControlStrategy)
    {
        addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8119,robotId);
        qDebug()<<"processNavSplineContinusInstruction error,不支持轨迹策略";
        return -1;
        //设置理想姿态
//        virtualLineBias->setMaccrumIdealMovingLineAngle(targetFrame.last().rotate);
//        tmpReturn=coordinateTrajectory->splineContinuousMove_vehiclePosition_maccrum(true,targetFrame,
//                                                                speedPercent,timeDurationResult,cntRatio);
    }

    pthread_mutex_unlock(&navMutex);
    if(1!=tmpReturn)
    {
        addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8101,robotId);
        qDebug()<<"processNavSplineContinusInstruction error,1!=tmpReturn";
        return -1;
    }


    pthread_mutex_lock(&navMutex);
    for(int i=0;i<pointCount;i++)
    {
       addTrajTime(moveParameterArry[i].lineNumber,moveParameterArry[i].stationId,moveParameterArry[i].pathId,timeDurationResult[i]);
       qDebug()<<"addTrajTime"<<timeDurationResult[i];
    }
    pthread_mutex_unlock(&navMutex);

    return 1;
}

int VirtualLineControl::processNavRotateInstruction(NavParameter moveParameter)
{
    qDebug()<<"VirtualLineControl::processNavRotateInstruction";

    printInformation(moveParameter,"processNavRotateInstruction");

//    virtualLineBias->setVehicleDirection(moveParameter.isPositiveDirection);//前进和倒退方向参数处理问题
    int tmpReturn=0;

    double timeDurationOut;

    pthread_mutex_lock(&navMutex);
    //axisGroup->resetCoordinateTraj(0);
//    updatePositionFrameCommand();
    VehiclePosition tmpPos,newTargetPoint;
    newTargetPoint=coordinateTrajectory->getVehiclePositionFeedback();
    dpointToVehiclePosition(moveParameter.targetPoint,tmpPos);
    newTargetPoint.rotate=tmpPos.rotate;
    QVector<VehiclePosition> tmpPointList;
    tmpPointList.append(newTargetPoint);
    coordinateTrajectory->setPathInformation(tmpPointList,moveParameter.targetPoint.dpIndex,E_NAV_COMMAND_ROTATE);
    coordinateTrajectory->synchronizePositionCommandWithFeedback();
    double tmpDistance,tmpAngle;
    coordinateTrajectory->getAbsoluteVehiclePositionLeft(tmpDistance,tmpAngle);


    tmpReturn=coordinateTrajectory->lineMove_only_rotation(true,newTargetPoint,
                                                           moveParameter.velocity,timeDurationOut);


    pthread_mutex_unlock(&navMutex);
    if(1!=tmpReturn)
    {
        addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8100,robotId);
//        qDebug()<<"processNavLineInstruction error,1!=tmpReturn";
        return -1;
    }


    pthread_mutex_lock(&navMutex);
    addTrajTime(moveParameter.lineNumber,moveParameter.stationId,moveParameter.pathId,timeDurationOut);
    pthread_mutex_unlock(&navMutex);

    return 1;
}


int VirtualLineControl::getCommandFromQueue(NavParameter & moveParameter)
{
    pthread_mutex_lock( &navMutex);//出队列前加锁
    if(commandQueue.isEmpty())
    {
        pthread_mutex_unlock( &navMutex);
        return 0;
    }
    moveParameter = commandQueue.dequeue();
//    if( 1<=moveParameter.moveType)
//    {
//        pthread_mutex_unlock( &navMutex);
//        if(2==debugflag)
//        {
//            qDebug()<< "error,  VirtualLineControl::getCommandFromQueue";
//        }

//        addMsg(ENUM_MSG_ERROR, "VirtualLineControl", "VirtualLineControl", 5019, -1);
//        return -1;
//    }
    pthread_mutex_unlock( &navMutex);


    return 1;
}

int VirtualLineControl::processLoopRealTime()
{

//    setPositionFrameFeedback(VehiclePosition vehiclePositionIn);
    return moveOneStep();
}

int VirtualLineControl::stop()
{
    resetAll();

//    if(E_VIRTUAL_LINE_CONTROL_RUN!=basicStatus)
//    {
//        resetAll();
//        return -1;
//    }

//    basicStatus=E_VIRTUAL_LINE_CONTROL_TOSTOP;
////    setTimeAxisScale(0,0.3);
//    basicStatus=E_VIRTUAL_LINE_CONTROL_STOPPING;

    return 1;
}


int VirtualLineControl::processLoopCommon()
{
    motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_LOOP;
    if(basicStatus==E_VIRTUAL_LINE_CONTROL_RUN || basicStatus==E_VIRTUAL_LINE_CONTROL_STOPPING ||
            basicStatus==E_VIRTUAL_LINE_CONTROL_TOSTOP || basicStatus==E_VIRTUAL_LINE_CONTROL_CEASING
            || basicStatus==E_VIRTUAL_LINE_CONTROL_TOCEASE)
     {
        if(0>calculateTrajectoryPath())
        {
            qDebug()<<"VirtualLineControl 0>calculateTrajectoryPath()";
            stop();
        }


     }
    else if(basicStatus==E_VIRTUAL_LINE_CONTROL_STOPPED)
    {

    }

    return 1;
}

int VirtualLineControl::resetTrajTime()
{
    pthread_mutex_lock(&navMutex);
    lineQueue.clear();
    stationIdQueue.clear();
    pathIdQueue.clear();
    timeDurationQueue.clear();
//    currentLine=ENUM_LINE_INITIAL;
    isCurrentLineFinished=false;
    currentLineRemainingTime=-1;
    currentLineTimeDuration=-1;
    lineTimeCount=0;
    lineTimeCountOld=-1;
    exceedTimeCount=0;
    updateTrajStage=0;
    if(0)//宏友要求报错保留站点信息
    {
        pathMovingInfo.passedStationId=-1;
        pathMovingInfo.nextStationId=-1;
        pathMovingInfo.currentStationId=-1;
        pathMovingInfo.passedPathId=-1;
        pathMovingInfo.nextPathId=-1;
        pathMovingInfo.currentPathId=-1;
    }

    pthread_mutex_unlock(&navMutex);
    return 1;
}

int VirtualLineControl::timeAxisMoveOneStep()
{
    motionDebugInfo.realThreadStage=E_REAL_THREAD_TIME_AXIS_MOVE_ONE_STEP;

    pthread_mutex_lock(&navMutex);

    int returnResult=timeAxis->moveOneStep();
    double timeAxisScaleOut=timeAxis->positionCommand;

    //更新所有轴timeScale;
    coordinateTrajectory->setTimeAxisScale(timeAxis->positionCommand);

    virtualAxis->setTimeAxisScale(timeAxis->positionCommand);

    pthread_mutex_unlock(&navMutex);
    motionDebugInfo.timeAxisScale0=timeAxisScaleOut;
    return returnResult;
}

int VirtualLineControl::setControlSpeedRatio(double controlSpeedRatioIn,double time)
{
    qDebug()<<"VirtualLineControl::setControlSpeedRatio"<<controlSpeedRatioIn<<time
           <<"pidSpeedRatio"<<pidSpeedRatio<<"safeSpeedRatio"<<safeSpeedRatio
             <<"adjustSpeedRatio"<<adjustSpeedRatio;
//    if(controlSpeedRatio==controlSpeedRatioIn)//reset后，会导致时间轴没有被重新设置
//    {
//        return -1;
//    }
    controlSpeedRatio=controlSpeedRatioIn;
    totalSpeedRatio=controlSpeedRatio*pidSpeedRatio*safeSpeedRatio*adjustSpeedRatio;
    setTimeAxisScale(totalSpeedRatio,time);
    return 1;
}

int VirtualLineControl::setAdjustSpeedRatio(double adjustSpeedRatioIn, double time)
{
    if(adjustSpeedRatio==adjustSpeedRatioIn)
    {
        return -1;
    }
    adjustSpeedRatio=adjustSpeedRatioIn;
    totalSpeedRatio=controlSpeedRatio*pidSpeedRatio*safeSpeedRatio*adjustSpeedRatio;
    setTimeAxisScale(totalSpeedRatio,time);
    return 1;
}

int VirtualLineControl::setSafeSpeedRatio(double safeSpeedRatioIn,double time)
{

    //如果激光丢失，不允许设置大于０
    if(true==isLaserLose && safeSpeedRatioIn>0.0001)
    {
        qDebug()<<"setSafeSpeedRatio failed, true==isLaserLose";
        return -2;
    }
    if(safeSpeedRatio==safeSpeedRatioIn)
    {
        return -1;
    }
//    qDebug()<<"22222222222222222 setSafeSpeedRatio";
    //如果恢复，将时间轴回退。
//    coordinateTrajectory->revertTime(0.2);//退回时间ｓ.应用于agv壁障。会卡顿。
    safeSpeedRatio=safeSpeedRatioIn;
    totalSpeedRatio=controlSpeedRatio*pidSpeedRatio*safeSpeedRatio*adjustSpeedRatio;
    setTimeAxisScale(totalSpeedRatio,time);
//    qDebug()<<"VirtualLineControl::setSafeSpeedRatio"<<safeSpeedRatioIn<<time;
    return 1;
}



bool VirtualLineControl::isServoOn()
{
    return isServOn;
}

int VirtualLineControl::moveOneStep()//virtualLineControl在时间轴为０的时候，会不更新updateTrajectory,导致调试信息显示不正确。
{
    //时间分析
    struct timespec startTime,endTime1,endTime2,endTime3,endTime4,endTime5,endTime6
            ,endTime5a,endTime5b,endTime5c;
    clock_gettime(CLOCK_MONOTONIC, &startTime);
    double currentConsumed;



    motionDebugInfo.realThreadStage=E_REAL_THREAD_IN;
   if(E_VIRTUAL_LINE_CONTROL_CEASED==basicStatus||E_VIRTUAL_LINE_CONTROL_STOPPED==basicStatus
           || E_VIRTUAL_LINE_CONTROL_INITIAL==basicStatus)
   {
       motionDebugInfo.realThreadStage=E_REAL_THREAD_MOVE_ONE_STEP_DISABLE;
       virtualLineBias->setPidStop(true);
       barrierProtectRatio=1.0;
       return -1;
   }

   //check servon state
   if(false==isServoOn())
   {
       qDebug()<<"VirtualLineControl::moveOneStep() error occureed! false==isServoOn()";
       addMsg(ENUM_MSG_ERROR,"MotionServer","VirtualLineControl",8103,robotId);
       motionDebugInfo.realThreadStage=E_REAL_THREAD_MOVE_ONE_STEP_DISABLE;
       virtualLineBias->setPidStop(true);
       return -1;
   }

   timeAxisMoveOneStep();

   pthread_mutex_lock(&navMutex);
   double composedLineVel=sqrt(currentVehicleVelocity.x_move*currentVehicleVelocity.x_move+
                               currentVehicleVelocity.y_move*currentVehicleVelocity.y_move);
   coordinateTrajectory->setAxisFeedbackVelocity(composedLineVel,0);
   int  moveOneResult=coordinateTrajectory->moveOneStep();
   pthread_mutex_unlock(&navMutex);

   clock_gettime(CLOCK_MONOTONIC, &endTime1);
   currentConsumed=1000000000*(endTime1.tv_sec-startTime.tv_sec)+endTime1.tv_nsec-startTime.tv_nsec;
   currentConsumed=currentConsumed/1000.0;
   if(100==debugFlag)
   {
       qDebug()<<"VirtualLineControl--- LoopRealTime VTime1:"<<currentConsumed;
   }

   double tmpRatio=1;
   if(E_VIRTUAL_LINE_CONTROL_RUN==basicStatus)
   {
        sensorProtectLoop(true,tmpRatio);//放在这里保证时间轴可以恢复正常．手工按下暂停，时间轴被设置为０，如果壁障恢复，会破坏暂停．
   }
   if(1==moveOneResult)
   {
      //如果还有轨迹，则传感器壁障检测
//      double tmpRatio;
//      sensorProtectLoop(true,tmpRatio);//?可能会导致：轨迹结束时，如果壁障停止，时间轴就一直停在０
      barrierProtectRatio=tmpRatio;

      virtualLineBias->setPidStop(false);
      //细分偏差计算类型
      E_MOTION_ID tmpId;
      pthread_mutex_lock(&navMutex);
      coordinateTrajectory->getSegmentMotionId(tmpId);
      pthread_mutex_unlock(&navMutex);
      laserSafetyDetect->setMotionStatus(tmpId);
      switch(tmpId)
      {
      case E_MOTION_ID_MOVE_WAIT_STEERING:
      {
          virtualLineBias->setCalculateBiasType(E_BIAS_TYPE_STEERING_WAIT);
          break;
      }
      case E_MOTION_ID_NONE:
      {
          virtualLineBias->setCalculateBiasType(E_BIAS_TYPE_NONE);
          break;
      }
      case E_MOTION_ID_ROTATE:
      {
          virtualLineBias->setCalculateBiasType(E_BIAS_TYPE_ROTATE);
          break;
      }
      case E_MOTION_ID_MOVE_LINE:
      {
          virtualLineBias->setCalculateBiasType(E_BIAS_TYPE_MOVE_LINE);
          break;
      }
      }

   }
   else if(-1==moveOneResult)
   {
       barrierProtectRatio=1.0;
   }
   else if(-2==moveOneResult)
   {
//       if(ENUM_TRAJECTORY_RUN==basicStatus)
//       {
//           basicStatus=ENUM_TRAJECTORY_TOSTOP;
//           addStopTraJectory();
//        }
//       quickStop();
//       addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",5005,robotId);
   }

   //因为时间颗粒０．１较大，可能导致时间比例达不到０．０００１就结束了，为了避免停不下来,
   //virtualLineControl在时间轴为０的时候，会不更新updateTrajectory,导致调试信息显示不正确。
   if(timeAxis->positionCommand<0.000001)
   {
       if(barrierProtectRatio<0.000001)
       {
           stopOutput();//0.1壁障时,他会导致无法恢复.
       }

       motionDebugInfo.realThreadStage=E_REAL_THREAD_OUT;
//       qDebug()<<"timeAxis->positionCommand<0.000001";//不是这里导致的运行卡死。
       if(E_VIRTUAL_LINE_CONTROL_CEASING==basicStatus)//暂停后更新到暂停完成的状态。
       {
           basicStatus=E_VIRTUAL_LINE_CONTROL_CEASED;
       }
       return -1;

   }
   else
   {
       virtualLineBias->setPidStop(false);
   }

   clock_gettime(CLOCK_MONOTONIC, &endTime2);
   currentConsumed=1000000000*(endTime2.tv_sec-startTime.tv_sec)+endTime2.tv_nsec-startTime.tv_nsec;
   currentConsumed=currentConsumed/1000.0;
   if(100==debugFlag)
   {
       qDebug()<<"VirtualLineControl----LoopRealTime VTime2:"<<currentConsumed;
   }


   updateTrajTime();

   double tmpAbsoluteDistance,tmpAbsoluteAngle,tmpDeltaDistance,tmpDeltaAngle;
   coordinateTrajectory->getAbsoluteVehiclePositionLeft(tmpAbsoluteDistance,tmpAbsoluteAngle);
   int tmpReturn=virtualLineBias->calculateBias(coordinateTrajectory->vehiclePositionCommandOld,
                                                coordinateTrajectory->vehiclePositionCommand
                                  ,tmpAbsoluteDistance,tmpAbsoluteAngle,tmpDeltaDistance,tmpDeltaAngle);
   if(-100==tmpReturn)
   {
       addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8112,robotId);
   }
   else    if(-103==tmpReturn)
   {
       addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8116,robotId);
   }
   else    if(-104==tmpReturn)
   {
       addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8117,robotId);
   }

   calculateVehicleFollowOffset(coordinateTrajectory->vehiclePositionCommand);//bug todo
   virtualLineBias->setActualFollowOffset(currentLineOffset,currentAngleOffset);
   checkFollowOffset(tmpDeltaDistance,tmpDeltaAngle);//检查pid输出的合理性。


   motionDebugInfo.realThreadStage=E_REAL_THREAD_MOVE_ONE_STEP_OUT;

   clock_gettime(CLOCK_MONOTONIC, &endTime3);
   currentConsumed=1000000000*(endTime3.tv_sec-startTime.tv_sec)+endTime3.tv_nsec-startTime.tv_nsec;
   currentConsumed=currentConsumed/1000.0;
   if(100==debugFlag)
   {
       qDebug()<<"VirtualLineControl-----LoopRealTime VTime3:"<<currentConsumed;
   }

   return 1;
}

int VirtualLineControl::updateTrajTime()
{
    motionDebugInfo.realThreadStage=E_REAL_THREAD_UPDATE_TRAJTIME;
    motionDebugInfo.isVelprofileEmpty=coordinateTrajectory->isVelProfileEmpty();

    pthread_mutex_lock(&navMutex);
    switch(updateTrajStage)
    {
    case 0:
    {
        //0.2=算法已取点, 0.5=正在运行，0.9=(已经到目标位置),  ,下一行+0.2(避免精度丢失,不用0.1)
        //取队列数据

        int returnFlag=0;
        int lineQueueSize=lineQueue.size();
        motionDebugInfo.lineQueueSize=lineQueueSize;
        if( lineQueueSize!=0 && -1==lineTimeCountOld)
        {
            currentLine=lineQueue.dequeue();
            currentLine+=0.5;
            currentLineTimeDuration=timeDurationQueue.dequeue();
            isCurrentLineFinished=false;
            currentLineRemainingTime=currentLineTimeDuration;
            updateTrajStage=1;

            if(isinf(currentLineRemainingTime))
            {
                addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8120,robotId);
            }

            //报错后，再次运行，保证passedStationId是对的
            if(-1!=pathMovingInfo.currentStationId)
            {
               pathMovingInfo.passedStationId=pathMovingInfo.currentStationId;
            }
            pathMovingInfo.currentStationId=-1;
            pathMovingInfo.nextStationId=stationIdQueue.dequeue();

            pathMovingInfo.passedPathId=pathMovingInfo.currentPathId;
            pathMovingInfo.currentPathId=pathIdQueue.dequeue();
            if(pathIdQueue.size()>0)
            {
               pathMovingInfo.nextPathId=pathIdQueue.front();
            }
            else
            {
                pathMovingInfo.nextPathId=-1;
            }



        }


        break;
    }
    case 1:
    {
        //单条完成判断
        lineTimeCountOld=lineTimeCount;

        lineTimeCount=coordinateTrajectory->getAccumulatedTime();

//        currentLineRemainingTime=currentLineTimeDuration-lineTimeCount;
//        currentLineRemainingTime<deltaTime;

        if( true==isLineFinished(currentLineTimeDuration)|| lineTimeCountOld>lineTimeCount || 0<exceedTimeCount)
        {
            qDebug()<<"virtualLine finished ,,, lineTimeCountOld"<<lineTimeCountOld<<"lineTimeCount"<<lineTimeCount
                   <<"exceedTimeCount"<<exceedTimeCount;
            updateTrajStage=2;
        }

        break;
    }
    case 2:
    {
        //等三个周期，使精确点的profile清空　。导致误判了当前为直线，导致出现几个错误的直线数据。不要等了。
         static int profileCleanWaitCount=0;
         profileCleanWaitCount++;
         if(profileCleanWaitCount<1)
         {
             break;
         }
         profileCleanWaitCount=0;

         if(0!=lineQueue.size())
         {//过渡点
            updateTrajStage=3;
         }
         else
         {//精确点
             updateTrajStage=4;
             coordinateTrajectory->clearTraj();
             qDebug()<<"updateTrajTime,,,coordinateTrajectory->resetTraj()";
         }

        break;
    }
    case 3:
    {
        isCurrentLineFinished=true;
        currentLine+=0.4;
        lineTimeCountOld=-1;
        lineTimeCount=0;
        if(false==isTrajectoryContiousRun)
        {
//                setTimeAxisScaleByTime(0.2);
        }
        motionDebugInfo.realThreadStage=E_REAL_THREAD_UPDATE_TRAJTIME_CNT_WAIT;
        updateTrajStage=0;
        pathMovingInfo.currentStationId=pathMovingInfo.nextStationId;

        break;
    }
    case 4:
    {
//        calculateBiasType=E_BIAS_TYPE_ABSOLUTE;效果不好，取消掉。
        virtualLineBias->setCalculateBiasType(E_BIAS_TYPE_IDILE);
        exceedTimeCount++;
        if(1==isTargetOffsetCheckOk())
        {//已经到达
            exceedTimeCount=0;
            isCurrentLineFinished=true;
            currentLine+=0.4;
            lineTimeCountOld=-1;
            lineTimeCount=0;
            virtualLineBias->setCalculateBiasType(E_BIAS_TYPE_NONE);
            updateTrajStage=0;
//                if(true==isMoveToPoint)
//                {
//                    isMoveToPoint=false;
//                    setBasicStatus(ENUM_TRAJECTORY_STOPPED);
//                }
            pathMovingInfo.currentStationId=pathMovingInfo.nextStationId;

        }
        else if(exceedTimeCount*deltaTime>targetWaitTime)//超过targetWaitTimeS 报错
        {//超时未到达
            exceedTimeCount=0;
            virtualLineBias->setCalculateBiasType(E_BIAS_TYPE_NONE);
//                quickStop();
            addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8102,robotId,targetWaitTime*1000);
            if(2==debugFlag)
            {
                qDebug()<< "target point FollowError";
            }
        }
        motionDebugInfo.realThreadStage=E_REAL_THREAD_UPDATE_TRAJTIME_FINE_WAIT;


        break;
    }
    default:
    {

    }
    }
    pthread_mutex_unlock(&navMutex);

    return 1;
}



int VirtualLineControl::isTrajectoryEmpty()
{
    pthread_mutex_lock(&navMutex);
    int returnResult;
    int commandQueueSize=commandQueue.size();
    if(0==commandQueueSize && 1==isAllVelProfileFinished())
    {
        returnResult= 1;
    }
    else
    {
        returnResult= 0;
    }
    pthread_mutex_unlock(&navMutex);

    return returnResult;
}

bool VirtualLineControl::isLineFinished(double currentLineTimeDuration)
{
    double lineTimeCount=coordinateTrajectory->getAccumulatedTime();
    currentLineRemainingTime=currentLineTimeDuration-lineTimeCount;
    //主要是为了和多段平滑兼容，有暂停后再次运行不加新运动指令的风险。
    if(currentLineRemainingTime<0.00000001 || 1==coordinateTrajectory->isVelProfileEmpty())
    {
        qDebug()<<"currentLineRemainingTime"<<currentLineRemainingTime<<"coordinateTrajectory->isVelProfileEmpty()"
               <<coordinateTrajectory->isVelProfileEmpty();
        return true;
    }
    return false;
}

int VirtualLineControl::setVehicleDirection(bool isPositiveIn)
{
    laserSafetyDetect->setVehicleDirection(isPositiveIn);
    return virtualLineBias->setVehicleDirection(isPositiveIn);
}


bool VirtualLineControl::isInFineMoving()
{
    bool returnResult=coordinateTrajectory->isInFineMoving();

    return returnResult;
}


float VirtualLineControl::getRunningNumber()
{
    return currentLine;
}

int VirtualLineControl::setRunningNumber_virtualLineControl(float numberIn)
{
    currentLine=numberIn;
    return 1;
}

int VirtualLineControl::getLineTimeDuration()
{
    return currentLineTimeDuration;
}

int VirtualLineControl::getLineRemainingTime()
{
    return currentLineRemainingTime;
}



int VirtualLineControl::getBias(BiasControlInfo &controlInfoOut)
{
    return virtualLineBias->getBias(controlInfoOut);
}

int VirtualLineControl::getFollowOffset(double &currentLineOffsetOut, double &currentAngleOffsetOut)
{
    currentLineOffsetOut=currentLineOffset;
    currentAngleOffsetOut=currentAngleOffset;
    return 1;
}

int VirtualLineControl::setPositionFeedback(VehiclePosition vehiclePositionIn, VehicleVelocity vehicleVelocityIn,
                                            QVector<double> steeringPositionIn)
{
    currentVehiclePosition_old=currentVehiclePosition;
    currentVehiclePosition=vehiclePositionIn;
    currentVehicleVelocity=vehicleVelocityIn;
    for(int i=0;i<steeringPositionIn.size();i++ )
    {
        steeringPositionFeedback[i]= steeringPositionIn[i];
    }

    virtualLineBias->setPositionFeedback(vehiclePositionIn,vehicleVelocityIn);
    return coordinateTrajectory->setVehiclePositionFeedback(vehiclePositionIn,steeringPositionIn);
}

int VirtualLineControl::setVehicleVelCommand(VehicleVelocity currentVehicleVelocityIn)
{
    return virtualLineBias->setVehicleVelCommand(currentVehicleVelocityIn);
}

int VirtualLineControl::getVirtuleLineDebugInfo(VirtualLineDebugInfo &infoOut)
{
    infoOut=motionDebugInfo;

    infoOut.deltaTime=deltaTime;
    infoOut.currentLineRemainingTime=currentLineRemainingTime;
    //infoOut.lineQueueSize;
    infoOut.basicStatus=basicStatus;
    //infoOut.commandQueueSize;
    infoOut.isVelprofileEmpty;
    //infoOut.timeAxisScale0
    //infoOut.commonThreadStage
    //infoOut.realThreadStage
//    infoOut.threadId=threadId;
    infoOut.motionServerMode=0;
    infoOut.currentLine=currentLine;
    infoOut.calculateBiasType=virtualLineBias->getCalculateBiasType();

    BiasControlInfo controlInfoOut;
    virtualLineBias->getBias( controlInfoOut);
    infoOut.deltaDistance=controlInfoOut.deltaDistanceX;
    infoOut.deltaDistance_y=controlInfoOut.deltaDistanceY;
    infoOut.deltaAngle=controlInfoOut.deltaAngle;
    infoOut.absoluteDistance=controlInfoOut.absoluteDistance;
    infoOut.absoluteAngle=controlInfoOut.absoluteAngle;
    infoOut.barrierProtectRatio=barrierProtectRatio;
    return 1;
}

int VirtualLineControl::getVehiclePositionCommand(VehiclePosition &vehiclePositionOut)
{
    pthread_mutex_lock( &navMutex);//出队列前加锁
    vehiclePositionOut=coordinateTrajectory->getVehiclePositionCommand();
    pthread_mutex_unlock( &navMutex);
}

int VirtualLineControl::setTimeAxisScale(double ratio, double timeDuration)
{
    timeAxisParameter.startRatio=ratio;
    timeAxisParameter.startDuration=timeDuration;
    //下面的语句会导致２次掉用setRatio的时候，立刻停止
    oldTimeAxisRatio=newTimeAxisRatio;
    newTimeAxisRatio=ratio;
    if(newTimeAxisRatio>oldTimeAxisRatio)
    {
        timeAxisStatus=ENUM_TIME_AXIS_ASCEND;
    }
    else if(newTimeAxisRatio==oldTimeAxisRatio)
    {
        timeAxisStatus=ENUM_TIME_AXIS_NORMAL;
    }
    else
    {
        timeAxisStatus=ENUM_TIME_AXIS_DESCEND;
    }
    pthread_mutex_lock(&navMutex);
    timeAxis->clearTraj();//保证立刻覆盖前面的旧轨迹
    timeAxis->splineZeroMoveByTime(ratio,timeDuration);
    pthread_mutex_unlock(&navMutex);
    return 1;
}

int VirtualLineControl::synchronizeMotorFeedbackToCalculatePosition()
{

    pthread_mutex_lock(&navMutex);
    coordinateTrajectory->resetAll();
    pthread_mutex_unlock(&navMutex);
    return 1;
}

int VirtualLineControl::isAllVelProfileFinished()
{
    if(0==coordinateTrajectory->isVelProfileEmpty())
    {
        return 0;
    }

    return  1;
}

int VirtualLineControl::isTargetOffsetCheckOk()
{

//    if(currentLineOffset<agv_allowLineOffset && currentAngleOffset<agv_allowAngleOffset) E_BIAS_TYPE_IDILE
    if(currentLineOffset<agv_allowLineOffset)
    {
        return 1;
    }
    else
    {
        return -1;
    }

//    if(currentLineOffset<agv_allowLineOffset )//for test
//    {
//        return 1;
//    }
//    else
//    {
//        return -1;
//    }
//    return 1;
}

int VirtualLineControl::getCommandQueueSize()
{
    int tmpSize;
    pthread_mutex_lock( &navMutex);//出队列前加锁
    tmpSize=commandQueue.size();
    pthread_mutex_unlock( &navMutex);
    return tmpSize;
}


int VirtualLineControl::calculateVehicleFollowOffset(VehiclePosition vehiclePositionCommand)
{
    if(2==debugFlag)
    {
        qDebug()<<"VirtualLineControl::calculateVehicleFollowOffset ";
    }
//    VehiclePosition tmpOldPos=coordinateTrajectory->vehiclePositionCommandOld;
    VehiclePosition tmpNewPos=vehiclePositionCommand;

    currentLineOffset=sqrt( (tmpNewPos.x-currentVehiclePosition.x)*(tmpNewPos.x-currentVehiclePosition.x));

//    currentLineOffset=sqrt((tmpNewPos.y-currentVehiclePosition.y)*(tmpNewPos.y-currentVehiclePosition.y)+
//                              (tmpNewPos.x-currentVehiclePosition.x)*(tmpNewPos.x-currentVehiclePosition.x));

    //bug, 直线段的角度偏差是不对的。
    currentAngleOffset=fabs(virtualLineBias->angleRotationJudgement(tmpNewPos.rotate,currentVehiclePosition.rotate));
    if(2==debugFlag)
    {
        qDebug()<<"currentLineOffset="<<currentLineOffset<<" currentAngleOffset="<<currentAngleOffset;
    }




    return 1;
}

int VirtualLineControl::stopOutput()
{

    virtualLineBias->stopOutput();
    return 1;
}


void VirtualLineControl::addMsg(int messageLevel, std::string componentName, std::string messageType
                              , int messageCode, int robotIdIn
                              , double parameter1, double parameter2, int parameter3, int parameter4
                                ,QString coment)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;

    string infomationStr;
    QString tmpStr;

    switch(messageCode)
    {
    case 8122:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",轨迹跟随距离误差(%1)小于允许值(%2)，解除降速保护　　！").arg(parameter1).arg(parameter2);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8121:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",轨迹跟随距离误差(%1)大于警告值(%2)，进入降速保护状态　！").arg(parameter1).arg(parameter2);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8120:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(", 路径时间无穷大错误！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8119:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(", 样条曲线不支持全向轮轨迹策略！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8118:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(", 不支持直线平滑，请使用样条曲线平滑！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8117:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(", 轨迹旋转激励持续过大！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8116:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(", 轨迹命令速度过慢！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8115:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",绝对轨迹跟随距离误差%1大于允许值%2　！").arg(parameter1).arg(parameter2);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8114:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",轨迹跟随角度误差(%1)大于允许值(%2)，超过限制次数　　！").arg(parameter1).arg(parameter2);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8113:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(", 轨迹类型(%1)不支持").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8112:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(", 直线轨迹运行，初始车的姿态角与直线角度偏差太大！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8111:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",轨迹跟随角度误差(%1)小于允许值(%2)，解除降速保护　　！").arg(parameter1).arg(parameter2);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8110:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",轨迹跟随角度误差(%1)大于警告值(%2)，进入降速保护状态　！").arg(parameter1).arg(parameter2);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8109:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",virtualLineBias初始化失败　！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8108:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",virtualLineConfig.xml文件读取失败　！")+coment;
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8107:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",要求车体前进行驶，但是当前车体方向与路径方向大于９０度，请先旋转好角度。　！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8106:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",要求车体倒退行驶，但是当前车体方向与路径方向大于９０度，请先旋转好角度。　！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8105:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",轨迹跟随角度误差(%1)大于允许值(%2)　！").arg(parameter1).arg(parameter2);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8104:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",轨迹跟随距离误差%1大于允许值%2　！").arg(parameter1).arg(parameter2);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8103:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",电机没有使能，不能运行！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8102:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",目标点超过时间%1毫秒未到达目标点允许误差范围！实际距离(%2)允许距离(%3), 实际角度(%4)允许角度(%5)").arg(parameter1)
                .arg(currentLineOffset).arg(agv_allowLineOffset).arg(180.0*currentAngleOffset/M_PI).arg(180.0*agv_allowAngleOffset/M_PI);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8101:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",车体平滑轨迹规划失败！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8100:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",车体直线轨迹规划失败！");
        infomationStr = tmpStr.toStdString();
        break;
    }




        default:
        {
            qDebug()<<"VirtualLineControl::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}


int VirtualLineControl:: initialTimeAxis()
{
    //时间轴初始化
    JointAttribute attribute;
    //trap用
//    attribute.maxAccelation=6;
//    //attribute.maxVelocity=1.7;
//    attribute.maxVelocity=3;
    //spline用
//        attribute.maxAccelation=6;
//        attribute.maxVelocity=2;

    //trap用
    attribute.maxAccelation=3;
    //attribute.maxVelocity=1.7;
//    attribute.maxVelocity=20;
    attribute.maxVelocity=200;

    attribute.maxTorque=10000;
    attribute.maxPosition=1.1;
    attribute.minPosition=-0.1;
    attribute.coupleFlag=0;
    attribute.coupleJoint=0;
    attribute.coupleReducer=0;
    attribute.jointType=0;
    attribute.pulsePerRound=100;
    attribute.externAxisFlag=0;
    attribute.gearRatio=1;
    attribute.jointName="timeAxis";
    attribute.mobile=false;
    attribute.processFollowError=2;
    attribute.screwPitch=1;
    attribute.targetFollowError=1;

    timeAxis=new AxisObject(robotId,attribute,motionMessage);
    timeAxis->axisIdByRobot=30000;
    timeAxis->setDeltaTime(deltaTime);

    return 1;
}

int VirtualLineControl::getTimeAxisStatus()
{
    if(1==timeAxis->isTrajComposeEmpty())
    {
       timeAxisStatus=ENUM_TIME_AXIS_NORMAL;
    }
    return timeAxisStatus;
}




int VirtualLineControl::setBasicStatus(E_VIRTUAL_LINE_CONTROL_STATUS statusIn)
{
    basicStatus=statusIn;
    return 1;
}


int VirtualLineControl::readConfigFile(QString configFilePathIn,QString &readComent)
{
    DomParser xmlParser;

    bool ok = xmlParser.openXml(configFilePathIn, QIODevice::ReadOnly );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, VirtualLineControl::readConfigFile open file error！";
       return -1;
    }

    QDomNode  inNode;



    //-------------read SafetySetting----------
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "SafetySetting", ok );
    if( !ok )
    {

        qDebug()<<"error, VirtualLineControl::readConfigFile node error！";
        return -1;
    }

    //节点ｒｅａｄ数据
    xmlParser.readXml( xmlParser.findSubNode(inNode, "isUltrasonicValid", ok), isUltrasonicValid  );
    if( !ok )
    {
        qDebug()<<"error, isUltrasonicValid node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "isPointCloudCheckValid", ok), isPointCloudCheckValid );
    if( !ok )
    {
        qDebug()<<"error, isPointCloudCheckValid node error！";
        return -1;
    }

    //-------------read CommonSetting----------
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "CommonSetting", ok );
    if( !ok )
    {
        qDebug()<<"error, VirtualLineControl::readConfigFile node error！";
        return -1;
    }

    //节点ｒｅａｄ数据
    xmlParser.readXml( xmlParser.findSubNode(inNode, "adjustRatioForAngleOffset", ok), adjustRatioForAngleOffset  );
    if( !ok )
    {
        qDebug()<<"error, adjustRatioForAngleOffset node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "adjustAngleOffset_startRatio", ok), adjustAngleOffset_startRatio );
    if( !ok )
    {
        qDebug()<<"error, adjustAngleOffset_startRatio node error！";
        return -1;
    }


    xmlParser.readXml( xmlParser.findSubNode(inNode, "adjustAngleOffset_endRatio", ok), adjustAngleOffset_endRatio );
    if( !ok )
    {
        qDebug()<<"error, adjustAngleOffset_endRatio node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "adjustTimeForAngleOffset", ok), adjustTimeForAngleOffset );
    if( !ok )
    {
        qDebug()<<"error, adjustTimeForAngleOffset node error！";
        return -1;
    }

    //
    xmlParser.readXml( xmlParser.findSubNode(inNode, "adjustRatioForLineOffset", ok), adjustRatioForLineOffset  );
    if( !ok )
    {
        qDebug()<<"error, adjustRatioForLineOffset node error！";
        readComent+=" adjustRatioForLineOffset";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "adjustLineOffset_startRatio", ok), adjustLineOffset_startRatio );
    if( !ok )
    {
        qDebug()<<"error, adjustLineOffset_startRatio node error！";
        readComent+=" adjustLineOffset_startRatio";
        return -1;
    }


    xmlParser.readXml( xmlParser.findSubNode(inNode, "adjustLineOffset_endRatio", ok), adjustLineOffset_endRatio );
    if( !ok )
    {
        qDebug()<<"error, adjustLineOffset_endRatio node error！";
        readComent+=" adjustLineOffset_endRatio";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "adjustTimeForLineOffset", ok), adjustTimeForLineOffset );
    if( !ok )
    {
        qDebug()<<"error, adjustTimeForLineOffset node error！";
        readComent+=" adjustTimeForLineOffset";
        return -1;
    }


    xmlParser.readXml( xmlParser.findSubNode(inNode, "noTransMoveDistance", ok), noTransMoveDistance );
    if( !ok )
    {
        qDebug()<<"error,  noTransMoveDistance node error！";
        readComent+=" noTransMoveDistance";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "noRotateMoveAngle", ok), noRotateMoveAngle );
    if( !ok )
    {
        qDebug()<<"error,  noRotateMoveAngle node error！";
        readComent+=" noRotateMoveAngle";
        return -1;
    }



    xmlParser.closeXml( );

    return 1;
}

int VirtualLineControl::sensorProtectLoop(bool isDetectLengthVariable,double &speedRatioOut)
{
    calculateSensorProtect(isDetectLengthVariable,speedRatioOut);
    static int normalStatusCount=0;
//    qDebug()<<"totalSafeSpeedRatio"<<totalSafeSpeedRatio;
    if(speedRatioOut<0.01)
    {
        setSafeSpeedRatio(speedRatioOut,0.2);

    }
    else if(speedRatioOut<1)
    {
//        double standardSlowTime=1.5;
//        double tmpCompareTime=calculateSlowDownTime();
//        if(tmpCompareTime<standardSlowTime)
//        {
//           setSafeSpeedRatio(totalSafeSpeedRatio,tmpCompareTime);
//           qDebug()<<"slow down use tmpCompareTime"<<tmpCompareTime;
//        }
//        else
//        {
//            setSafeSpeedRatio(totalSafeSpeedRatio,standardSlowTime);
//        }
        //以前的容易撞到脚
        setSafeSpeedRatio(speedRatioOut, laserSafetyDetect->getSafetySlowTime());


    }
    else
    {
        //定时恢复，如果壁障传感器抖动，会导致运动抖动。关键原因还是低速调整Ｐ太大了，导致震荡。
         if(100<normalStatusCount)
         {
             normalStatusCount++;
         }
         else
         {
             normalStatusCount=0;
             setSafeSpeedRatio(speedRatioOut,5.0);//３Ｓ恢复时，导致撞到脚
         }


    }
//    if(2==debugflag)
//    {
//        qDebug()<< "sensorProtectLoop:speedRatioOut"<<speedRatioOut<< "safeSpeedRatio"<<safeSpeedRatio
//                   << "controlSpeedRatio"<<controlSpeedRatio<< "adjustSpeedRatio"<<adjustSpeedRatio
//                   << "pidSpeedRatio"<<pidSpeedRatio<<"slowtime"<<laserSafetyDetect->getSafetySlowTime();
//    }
    return 1;

}

int VirtualLineControl::calculateSensorProtect(bool isDetectLengthVariable, double &speedRatioOut)
{

    double totalSafeSpeedRatio=1;
    double safeSpeedRatio_ultrasonic=1;
    if(1==isUltrasonicValid)
    {
        if(isDetectLengthVariable)
        {
            ultrasonicSensorProtectLoop(currentVehicleVelocity,safeSpeedRatio_ultrasonic);
        }
        else
        {
            VehicleVelocity tmpVel;
            tmpVel.x_move=0;
            tmpVel.y_move=0;
            tmpVel.z_rotate=0;
             ultrasonicSensorProtectLoop(tmpVel,safeSpeedRatio_ultrasonic);
        }

        if(safeSpeedRatio_ultrasonic<1)
        {
//            qDebug()<<"ultrasonicSensor barrier,,,safeSpeedRatio_ultrasonic="<<safeSpeedRatio_ultrasonic;
        }
    }

    //速度快时，激光雷达可能有异常数据，导致错误减速。解决：前面的情况是因为以前的区域计算有bug.
    double safeSpeedRatio_poindCloud=1;
    static double safeSpeedRatio_poindCloud_old=1;
    static double safeSpeedRatio_poindCloud_old_old=1;

    if(1==isPointCloudCheckValid)
    {
        if(isDetectLengthVariable)
        {
            laserSafetyDetect->setVehicleVelocity(currentVehicleVelocity);
        }
        else
        {
            VehicleVelocity tmpVel;
            tmpVel.x_move=0;
            tmpVel.y_move=0;
            tmpVel.z_rotate=0;
            laserSafetyDetect->setVehicleVelocity(tmpVel);
        }


        if(1>laserSafetyDetect->getSafetyRatioOut() && 1>safeSpeedRatio_poindCloud_old
                && 1>safeSpeedRatio_poindCloud_old_old)
//        if(1>laserSafetyDetect->getSafetyRatioOut() )
        {
             safeSpeedRatio_poindCloud=laserSafetyDetect->getSafetyRatioOut();
        }
        else
        {
            safeSpeedRatio_poindCloud=1;
        }
        safeSpeedRatio_poindCloud_old_old=safeSpeedRatio_poindCloud_old;
        safeSpeedRatio_poindCloud_old=laserSafetyDetect->getSafetyRatioOut();

        if(safeSpeedRatio_poindCloud<1)
        {
//            qDebug()<<"laserScan barrier,,,safeSpeedRatio_poindCloud="<<safeSpeedRatio_poindCloud;
        }
    }


    if(totalSafeSpeedRatio>safeSpeedRatio_ultrasonic)
    {
        totalSafeSpeedRatio=safeSpeedRatio_ultrasonic;
    }
    if(totalSafeSpeedRatio>safeSpeedRatio_poindCloud)
    {
        totalSafeSpeedRatio=safeSpeedRatio_poindCloud;
    }


    speedRatioOut=totalSafeSpeedRatio;

    return 1;
}

double VirtualLineControl::calculateSlowDownTime()
{
    double composedLineVel=sqrt(currentVehicleVelocity.x_move*currentVehicleVelocity.x_move+
                                currentVehicleVelocity.y_move*currentVehicleVelocity.y_move);
    double tmpLineTime=composedLineVel/lineAcc_slowdown;
    //有时候不动，这里的问题码?出现这种情况好像只修改过壁障，0.01改为0.3
    if(tmpLineTime<0.3)//如过比0.1小，可能会导致时间轴走不了
    {
        tmpLineTime=0.3;
    }
    double tmpRotateTime=currentVehicleVelocity.z_rotate/rotateAcc_slowdown;
    if(tmpRotateTime<0.3)
    {
        tmpRotateTime=0.3;
    }
    if(tmpLineTime<tmpRotateTime)
    {
//        qDebug()<<"calculateSlowDownTime use tmpRotateTime"<<tmpRotateTime
//               <<",tmpLineTime"<<tmpLineTime;
        return tmpRotateTime;
    }
    else
    {
//        qDebug()<<"calculateSlowDownTime use tmpLineTime"<<tmpLineTime
//                  <<",tmpRotateTime"<<tmpRotateTime;
        return tmpLineTime;
    }




}

int VirtualLineControl::ultrasonicSensorProtectLoop(VehicleVelocity vehicleVelocityIn, double &safeSpeedRatioOut)
{
    if(NULL==g_ultrasonicSensor)
    {
        return 0;
    }
    double barrierSlowDownDistanceOut, barrierSlowDownRatioOut,barrierStopDistanceXOut,barrierStopDistanceYOut;
    laserSafetyDetect->getCollisionParameter(barrierSlowDownDistanceOut, barrierSlowDownRatioOut,
                                             barrierStopDistanceXOut,barrierStopDistanceYOut);
    g_ultrasonicSensor->setNewCollisionParameter(barrierSlowDownDistanceOut, barrierSlowDownRatioOut,
                                                 barrierStopDistanceXOut);
    //todo麦克纳姆轮，可能同时友前后和左右的壁障。
    return g_ultrasonicSensor->ultrasonicSensorProtectLoop(laserSafetyDetect->getDetectMode(),safeSpeedRatioOut);
}

int VirtualLineControl::initialPointCloudSafeDetect()
{
    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    QString safeConfigFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/pointCloudSafeRatio.xml";
    QString borderConfigFile=D_TCRCONTROLLER_DATA_PATH;
    borderConfigFile+=D_ROS_CONFIG_FILE;
    borderConfigFile+="carBorder.xml";
    laserSafetyDetect=new LaserSafetyDetect(robotId,motionMessage,safeConfigFile,borderConfigFile,ioController);
    return 1;
}
#define D_PROTECT_COUNT 30
int VirtualLineControl::checkFollowOffset(double deltaDistanceIn,double deltaAngleIn)
{
    //1)距离偏差输出的判断，大于compareVel才检查，而且允许偏差和速度关联
    double composedLineVel=sqrt(currentVehicleVelocity.x_move*currentVehicleVelocity.x_move+
                                currentVehicleVelocity.y_move*currentVehicleVelocity.y_move);
    double compareVel=0.01;

    double new_allowOffset;
    if(composedLineVel>compareVel)
    {
        new_allowOffset=agv_allowLineMovingOffset+(composedLineVel-compareVel)*agv_allowLineMovingOffset_ratio;
    }
    else
    {
        new_allowOffset=agv_allowLineMovingOffset;
    }

    static int tmp_delta_distance_count=0;
    if(fabs(deltaDistanceIn)>new_allowOffset)
    {
        tmp_delta_distance_count++;
    }
    else
    {
        tmp_delta_distance_count=0;
    }
    if(tmp_delta_distance_count>4 && E_BIAS_TYPE_MOVE_LINE==virtualLineBias->getCalculateBiasType())
    {
//        addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8104,robotId,deltaDistanceIn,
//               new_allowOffset);
//        qDebug()<< "distance FollowError";
//        virtualLineBias->stopOutput();
//        return -1;
    }

    //2)实际距离偏差判断,旋转的时候不进行距离偏差判断。
//    if(fabs(currentLineOffset)>new_allowOffset && E_BIAS_TYPE_MOVE_LINE==virtualLineBias->getCalculateBiasType())
//    {
//        addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8115,robotId,currentLineOffset,
//               new_allowOffset);
//        qDebug()<< "distance FollowError";
//        virtualLineBias->stopOutput();
//        return -1;
//    }


    //2)检查角度偏差和保护。　偶尔会有微小的速度抖动，导致容易报错
    static int tmp_delta_angle_count=0;
    if(fabs(deltaAngleIn)>agv_allowAngleMovingOffset && fabs(deltaDistanceIn)>0.05)
    {
        tmp_delta_angle_count++;
    }
    else
    {
       tmp_delta_angle_count=0;
    }
    if(tmp_delta_angle_count>4)
    {
        addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8105,robotId,deltaAngleIn*180/M_PI,
               agv_allowAngleMovingOffset*180/M_PI);
        if(2==debugFlag)
        {
            qDebug()<< "angle FollowError";
        }
        virtualLineBias->stopOutput();
        return -2;
    }

    static bool isInProtectStatus=false;
    //如果角度偏差大于adjustAngleOffset_startRatio％，开始降低速度. 只在平滑的时候做限速保护。
    static int protectTimeCount;//减速保护，必须持续一定时间。
    if(E_BIAS_TYPE_MOVE_LINE==virtualLineBias->getCalculateBiasType())
    {
        if(fabs(deltaAngleIn)>adjustAngleOffset_startRatio*agv_allowAngleMovingOffset && false==isInProtectStatus)
        {
            protectTimeCount=D_PROTECT_COUNT;
            qDebug()<<"过程角度误差过大　deltaAngleIn"<<deltaAngleIn<<"　允许误差"
                   <<adjustAngleOffset_startRatio*agv_allowAngleMovingOffset;
            setAdjustSpeedRatio(adjustRatioForAngleOffset,adjustTimeForAngleOffset);
            addMsg(ENUM_MSG_WARNING,"VirtualLineControl","VirtualLineControl",8110,robotId,deltaAngleIn*180/M_PI,
                   adjustAngleOffset_startRatio*agv_allowAngleMovingOffset*180/M_PI);
            isInProtectStatus=true;
        }
    }

    if(true==isInProtectStatus)
    {
        protectTimeCount--;
    }

    //如果角度偏差小于adjustAngleOffset_endRatio％，开始恢复速度
//    if(fabs(deltaAngleIn)<adjustAngleOffset_endRatio*agv_allowAngleMovingOffset && fabs(deltaDistanceIn)>0.05)
    if(fabs(deltaAngleIn)<adjustAngleOffset_endRatio*agv_allowAngleMovingOffset && true==isInProtectStatus
            && protectTimeCount<0)
    {
        qDebug()<<"过程角度误差恢复正常　deltaAngleIn"<<deltaAngleIn<<"　允许误差"
               <<adjustAngleOffset_startRatio*agv_allowAngleMovingOffset;
        setAdjustSpeedRatio(1,adjustTimeForAngleOffset);
        addMsg(ENUM_MSG_WARNING,"VirtualLineControl","VirtualLineControl",8111,robotId,deltaAngleIn*180/M_PI,
               adjustAngleOffset_endRatio*agv_allowAngleMovingOffset*180/M_PI);
        isInProtectStatus=false;
    }
    //聿冠出现走直线震荡，因为deltaAngleIn在过误差和０之间变。

    //３）角度偏差报错保护。对于角度误差超过一定次数进行报错。急停后，车再次启动，需要进行保护。这个检查太多余了
//    static int angleOffsetErrorCount=0;
//    if(fabs(deltaAngleIn)>adjustAngleOffset_startRatio*agv_allowAngleMovingOffset )
//    {
//        angleOffsetErrorCount++;
//    }
//    else
//    {
//        angleOffsetErrorCount=0;
//    }
//    if(angleOffsetErrorCount>50)
//    {
//        addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8114,robotId,deltaAngleIn*180/M_PI,
//               adjustAngleOffset_startRatio*agv_allowAngleMovingOffset*180/M_PI);
//    }

    //进行跟随距离误差的降低速度保护．
    static bool isInProtectLineOffsetStatus=false;
    //如果角度偏差大于adjustAngleOffset_startRatio％，开始降低速度. 只在平滑的时候做限速保护。
    static int lineProtectTimeCount;//减速保护，必须持续一定时间。
    if(E_BIAS_TYPE_MOVE_LINE==virtualLineBias->getCalculateBiasType())
    {
        if(fabs(currentLineOffset)>adjustLineOffset_startRatio*new_allowOffset && false==isInProtectLineOffsetStatus)
        {
            lineProtectTimeCount=D_PROTECT_COUNT;
            qDebug()<<"过程距离误差过大　deltaDistanceIn"<<deltaDistanceIn<<"　允许误差"
                   <<new_allowOffset;
            setAdjustSpeedRatio(adjustRatioForLineOffset,adjustTimeForLineOffset);
            addMsg(ENUM_MSG_WARNING,"VirtualLineControl","VirtualLineControl",8121,robotId,currentLineOffset,
                   new_allowOffset);
            isInProtectLineOffsetStatus=true;
        }
    }

    if(true==isInProtectLineOffsetStatus)
    {
        lineProtectTimeCount--;
    }

    //如果偏差小于，开始恢复速度
    if(fabs(currentLineOffset)<adjustLineOffset_endRatio*new_allowOffset && true==isInProtectLineOffsetStatus
            && lineProtectTimeCount<0)
    {
        qDebug()<<"过程距离误差恢复　deltaDistanceIn"<<deltaDistanceIn<<"　允许误差"
               <<new_allowOffset;
        setAdjustSpeedRatio(1,adjustTimeForLineOffset);
        addMsg(ENUM_MSG_WARNING,"VirtualLineControl","VirtualLineControl",8122,robotId,currentLineOffset,
               new_allowOffset);
        isInProtectLineOffsetStatus=false;
    }
    return 1;

}

int VirtualLineControl::dpointToVehiclePosition(DPoint dpointIn, VehiclePosition &positionOut)
{
    positionOut.x=dpointIn.posX/1000.0;
    positionOut.y=dpointIn.posY/1000.0;
    //degree to rad
    positionOut.rotate=M_PI*dpointIn.posR/180.0;
    return 1;
}


